#include "mpu9250.h"
#include "inv_mpu.h"

short mmx,mmy,mmz = 0;
i2c_handle_type hi2cx_mpu;
mpu_handle_type mpu_data;

unsigned char MPU_Write_Len_test(unsigned char addr,unsigned char reg,unsigned char len);//,unsigned char *buf)
	

void i2c_lowlevel_init(i2c_handle_type* hi2c)
{
  gpio_init_type gpio_init_structure;

  if(hi2c->i2cx == MPU_I2Cx_PORT)
  {
    /* i2c periph clock enable */
    crm_periph_clock_enable(MPU_I2Cx_CLK, TRUE);
    crm_periph_clock_enable(MPU_I2Cx_SCL_GPIO_CLK , TRUE);
    crm_periph_clock_enable(MPU_I2Cx_SDA_GPIO_CLK , TRUE);

    /* gpio configuration */
    gpio_pin_mux_config(MPU_I2Cx_SCL_GPIO_PORT, MPU_I2Cx_SCL_GPIO_PinsSource, MPU_I2Cx_SCL_GPIO_MUX);

    gpio_pin_mux_config(MPU_I2Cx_SDA_GPIO_PORT, MPU_I2Cx_SDA_GPIO_PinsSource, MPU_I2Cx_SDA_GPIO_MUX);

    /* configure i2c pins: scl */
    gpio_init_structure.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init_structure.gpio_mode           = GPIO_MODE_MUX;
    gpio_init_structure.gpio_out_type       = GPIO_OUTPUT_OPEN_DRAIN;
    gpio_init_structure.gpio_pull           = GPIO_PULL_UP;

    gpio_init_structure.gpio_pins           = MPU_I2Cx_SCL_GPIO_PIN;
    gpio_init(MPU_I2Cx_SCL_GPIO_PORT, &gpio_init_structure);

    /* configure i2c pins: sda */
    gpio_init_structure.gpio_pins           = MPU_I2Cx_SDA_GPIO_PIN;
    gpio_init(MPU_I2Cx_SDA_GPIO_PORT, &gpio_init_structure);

    /* config i2c */
    i2c_init(hi2c->i2cx, 0x0F, MPU_I2Cx_CLKCTRL);

    i2c_own_address1_set(hi2c->i2cx, I2C_ADDRESS_MODE_7BIT, 0X55);
  }
}

u8 AK8963_dat = 0;
//初始化MPU9250
//返回值:0,成功
//    其他,错误代码
//_Bool MPU9250_Init(void)
//{
//	unsigned char res=0;
//	
//	
//	hi2cx_mpu.i2cx = MPU_I2Cx_PORT;
//	i2c_config(&hi2cx_mpu);
//	
//	MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位MPU9250
//	delay_ms(100);								//延时100ms
//	MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU9250
//	MPU_Set_Gyro_Fsr(3);					        	//陀螺仪传感器,±1000dps
//	MPU_Set_Accel_Fsr(0);					       	 	//加速度传感器,±2g
//	MPU_Set_Rate(100);						       	 	//设置采样率125Hz
//	MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00);   //关闭所有中断
//	MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
//	MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00);	//关闭FIFO
//	MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
//	res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG);  //读取MPU6500的ID
//	
//    if(res == MPU6500_ID1 || res==MPU6500_ID2) //器件ID正确
//    {
//        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01);  	//设置CLKSEL,PLL X轴为参考
//        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00);  	//加速度与陀螺仪都工作
//		    MPU_Set_Rate(50);								       	//设置采样率为50Hz   
//    }else return 1;
// 
//    res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA);    			//读取AK8963 ID   
//		
//    if(res==AK8963_ID)
//    {
//			MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X0f);
//			AK8963_dat = MPU_Read_Byte(MPU9250_ADDR,MAG_CNTL1);
//			MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X01);        //设置AK8963为单次测量模式
//			
//      //MPU_Get_Magnetometer(&mmx,&mmy, &mmz);	
//    }else return 1;

//    return 0;
//}

//³õÊ¼»¯MPU9250
//·µ»ØÖµ:0,³É¹¦
//    ÆäËû,´íÎó´úÂë
unsigned char data_t[10];
_Bool MPU9250_Init(void)
{
	u8 res=0;
	float pitch,roll,yaw; 
//	IIC_Init();     //³õÊ¼»¯IIC×ÜÏß
	
	hi2cx_mpu.i2cx = MPU_I2Cx_PORT;
	i2c_config(&hi2cx_mpu);
	
	MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//¸´Î»MPU9250
	delay_ms(100);  //ÑÓÊ±100ms
	MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//»½ÐÑMPU9250
	MPU_Set_Gyro_Fsr(3);					        	//ÍÓÂÝÒÇ´«¸ÐÆ÷,¡À2000dps
	MPU_Set_Accel_Fsr(0);					       	 	//¼ÓËÙ¶È´«¸ÐÆ÷,¡À2g
	MPU_Set_Rate(50);						       	 	//ÉèÖÃ²ÉÑùÂÊ50Hz
	MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00);   //¹Ø±ÕËùÓÐÖÐ¶Ï
	MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2CÖ÷Ä£Ê½¹Ø±Õ
	MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00);	//¹Ø±ÕFIFO
	MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INTÒý½ÅµÍµçÆ½ÓÐÐ§£¬¿ªÆôbypassÄ£Ê½£¬¿ÉÒÔÖ±½Ó¶ÁÈ¡´ÅÁ¦¼Æ
	res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG);  //¶ÁÈ¡MPU6500µÄID
	
//	unsigned char MPU_Write_Byte(unsigned char addr,unsigned char reg,unsigned char data)
//{
//	return i2c_memory_write(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|0, reg, &data, 0x01, 0xFFFF);
	
	
    if(res==MPU6500_ID1 || res==MPU6500_ID2) //Æ÷¼þIDÕýÈ·
    {
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01);  	//ÉèÖÃCLKSEL,PLL XÖáÎª²Î¿¼
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00);  	//¼ÓËÙ¶ÈÓëÍÓÂÝÒÇ¶¼¹¤×÷
		    MPU_Set_Rate(50);						       	//ÉèÖÃ²ÉÑùÂÊÎª50Hz   
    }else return 1;
 
//    res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA);    			//¶ÁÈ¡AK8963 ID   
//	
//    if(res==AK8963_ID)
//    {
//        MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11);		//ÉèÖÃAK8963Îªµ¥´Î²âÁ¿Ä£Ê½
//    }else return 1;

		mpu_dmp_get_data(&pitch,&roll,&yaw);
    return 0;
}





//设置MPU9250陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
unsigned char MPU_Set_Gyro_Fsr(unsigned char fsr)
{
	return MPU_Write_Byte(MPU9250_ADDR,MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU9250加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
unsigned char MPU_Set_Accel_Fsr(unsigned char fsr)
{
	return MPU_Write_Byte(MPU9250_ADDR,MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}

//设置MPU9250的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
unsigned char MPU_Set_LPF(unsigned short lpf)
{
	unsigned char data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return MPU_Write_Byte(MPU9250_ADDR,MPU_CFG_REG,data);//设置数字低通滤波器  
}

//设置MPU9250的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
unsigned char MPU_Set_Rate(unsigned short rate)
{
	unsigned char data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data = 1000/rate-1;
	data = MPU_Write_Byte(MPU9250_ADDR,MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    unsigned char buf[2]; 
    short raw;
	float temp;
	MPU_Read_Len(MPU9250_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((unsigned short)buf[0]<<8)|buf[1];  
    temp=21+((double)raw)/333.87;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
unsigned char MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    unsigned char buf[6],res; 
	res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gx=((unsigned short)buf[0]<<8)|buf[1];  
		*gy=((unsigned short)buf[2]<<8)|buf[3];  
		*gz=((unsigned short)buf[4]<<8)|buf[5];
	} 	
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
unsigned char MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    unsigned char buf[6],res;  
	res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*ax=((unsigned short)buf[0]<<8)|buf[1];  
		*ay=((unsigned short)buf[2]<<8)|buf[3];  
		*az=((unsigned short)buf[4]<<8)|buf[5];
	} 	
    return res;;
}

void delay_mm(void)
{
	uint16_t j,p;
	for(j=0;j<1000;j++)
	{
		for(p=0;p<1000;p++)
		{
			;
		}
	}
}

//得到磁力计值(原始值)
//mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 bit0 = 0;
unsigned char MPU_Get_Magnetometer(short *mx,short *my,short *mz)
{
    unsigned char Magnet_buf[6],res,dat; 
	
//	MPU_Write_Byte(AK8963_ADDR,0x11,AK8963_dat);
//	dat = MPU_Read_Byte(MPU9250_ADDR,0x11);
     	
	bit0 = MPU_Read_Byte(MPU9250_ADDR,0x02);
	
//	if(bit0 & 0x01)
//	{
		delay_ms(10);
		MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,Magnet_buf);
//		if(res==0)
//		{
			*mx=((unsigned short)Magnet_buf[1]<<8)|Magnet_buf[0];  
			*my=((unsigned short)Magnet_buf[3]<<8)|Magnet_buf[2];  
			*mz=((unsigned short)Magnet_buf[5]<<8)|Magnet_buf[4];
//		}
 //}
    MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X01); //AK8963每次读完以后都需要重新设置为单次测量模式
  
    return res;
}


//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
unsigned char MPU_Write_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
{
	return i2c_memory_write(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|0, reg, buf, len, 0xFFFF);
} 


unsigned char datbuf[10] = {0};
unsigned char MPU_Write_Len_test(unsigned char addr,unsigned char reg,unsigned char len)//,unsigned char *buf)
{
	i2c_memory_write(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|0, reg, datbuf, len, 0xFFFF);
	return 1;
} 

//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
unsigned char MPU_Read_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
{ 
	return i2c_memory_read(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|0, reg, buf, len, 0xFFFF);
}

//IIC写一个字节 
//devaddr:器件IIC地址
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
unsigned char MPU_Write_Byte(unsigned char addr,unsigned char reg,unsigned char data)
{
	return i2c_memory_write(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|0, reg, &data, 0x01, 0xFFFF);
}

//IIC读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
unsigned char MPU_Read_Byte(unsigned char addr,unsigned char reg)
{
	unsigned char dat = 0;
	i2c_memory_read(&hi2cx_mpu, I2C_MEM_ADDR_WIDIH_8, (addr<<1)|1, reg, &dat, 0x01, 0xFFFF);
	return dat;
}
